The present invention relates to articulated mechanisms, such as but not limited to robot arms, and more particularly to a predictor for determining joint positions and rates for multiple joints based on a desired movement of one or more other joints, such as is necessary for control or design of articulated mechanisms.
Articulated mechanisms such as a robot arm can be understood as a set of rigid links connected to each other by joints. The joints for a robot arm may be associated with actuators such as motors to move the joints and sensors providing an indication of joint position. Common joints include so-called revolute joints acting like a hinge to provide rotation about an axis and prismatic joints acting like a slide to provide translation along an axis. These types of joints are termed “lower-pair joints”. The absolute or global position of each joint is determined by the previous joints in the form of a kinematic chain where one joint is attached to and thus referenced to the “ground” which provides a global reference frame.
A typical robot arm, for example, might provide for six joints of various kinds describing six degrees of freedom. For example, a robot arm may swivel about a waist joint which supports a pivoting shoulder joint which in turn supports a pivoting elbow joint, which attaches to an extension for an arm joint allowing forearm rotation, in turn attaches to a pivoting and swiveling wrist joint the latter of which is attached to a tool such as an actuatable clamp, a paint sprayer, or welding gun, or any other such tool fixed to one link of that robot arm.
The position of the robot arm and its tool fixed to the terminal link may be readily calculated by using standard trigonometric formulas applied to each joint starting at the joint attached to ground to successively determine the locations and orientations of the later joints along with their intermediate links until the “effector” link is reached (sometimes but not always the last link in the chain). This calculation is termed the “forward kinematic” problem and requires knowledge of the axis values (e.g. joint angles) of each joint and of the structure of the mechanism (e.g. the link lengths, relative orientation of joints at each end of a link, types of joints etc.). This information, which describes the articulated mechanism, defines a “kinematic equation”. The kinematic equation may be readily expressed in a matrix form such as the Denavit-Hartenberg (D-H) matrix. That D-H matrix fully characterizes the translation and orientation of a link in an articulated mechanism from a reference position.
An important robot control problem involves determining the angles or extensions of the various joints necessary to position the robot tool (effector) in a desired orientation and location. This is the so-called “inverse kinematics” problem and again requires finding a solution to the kinematic equation meeting the desired tool position. In one approach to the inverse kinematics problem, the kinematic equation is formed into a loop equation by adding a final fictitious joint connection between the tool link and the work surface fixed to ground so that the entire mechanism satisfies a closed-loop kinematic equation. That fictitious joint constrains the spatial motion to be taken by the tool over one segment of a desired robot work task. The desired tool positions and orientations at the start and end of that segment of the work task define the type, location, and movement range of this final fictitious joint.
Some industrial robots, especially those with a “spherical wrist” having the roll, yaw, and pitch axes intersecting on a common center, have a mathematically closed-form inverse kinematic solution. For other robots, especially those with a non-spherical wrist or incorporating redundant joints to achieve a high level of dexterity, determining motion of the effector as part of the inverse kinematics problem from this loop equation may be done by differentiating the loop equation to determine its local slope which may be used to estimate the necessary joint movements in a process roughly analogous to the Newton Raphson method. Determining higher derivatives of a complicated loop equation may be done symbolically with significant manual effort or with a symbolic algebra software package, but quickly becomes intractable owing to the rapid growth in the number of terms that precludes checking such symbolic expressions for correctness or their practical implementation in computer software.